
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sim_quadplane.c
  * @author     baiyang
  * @date       2022-12-9
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sim_quadplane.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
void sim_quadplane_update(sim_aircraft_t aircraft, const struct sitl_input *input);
/*----------------------------------variable----------------------------------*/
struct sim_aircraft_ops quadplane_ops = {.set_start_location = NULL,
                                           .set_interface_ports = NULL,
                                           .update = sim_quadplane_update,
                                           .set_config = NULL,
                                           .rangefinder_beam_width = NULL,
                                           .perpendicular_distance_to_rangefinder_surface = NULL,
                                           .on_ground = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sim_quadplane_ctor(sim_quadplane_t quadplane, const char *frame_str)
{
    sim_aircraft_t aircraft = &quadplane->plane.aircraft;
    sim_plane_t plane       = &quadplane->plane;

    memset(quadplane, 0, sizeof(struct sim_quadplane));

    sim_plane_ctor(&quadplane->plane, &quadplane_ops, frame_str);

    // default to X frame
    const char *frame_type = "x";
    uint8_t motor_offset = 4;

    aircraft->ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;

    if (strstr(frame_str, "-octa-quad")) {
        frame_type = "octa-quad";
    } else if (strstr(frame_str, "-octaquad")) {
        frame_type = "octa-quad";
    } else if (strstr(frame_str, "-octa")) {
        frame_type = "octa";
    } else if (strstr(frame_str, "-hexax")) {
        frame_type = "hexax";
    } else if (strstr(frame_str, "-hexa")) {
        frame_type = "hexa";
    } else if (strstr(frame_str, "-plus")) {
        frame_type = "+";
    } else if (strstr(frame_str, "-y6")) {
        frame_type = "y6";
    } else if (strstr(frame_str, "-tri")) {
        frame_type = "tri";
    } else if (strstr(frame_str, "-tilttrivec")) {
        frame_type = "tilttrivec";
        // fwd motor gives zero thrust
        plane->thrust_scale = 0;
    } else if (strstr(frame_str, "-tilthvec")) {
        frame_type = "tilthvec";
    } else if (strstr(frame_str, "-tilttri")) {
        frame_type = "tilttri";
        // fwd motor gives zero thrust
        plane->thrust_scale = 0;
    } else if (strstr(frame_str, "firefly")) {
        frame_type = "firefly";
        // elevon style surfaces
        plane->elevons = true;
        // fwd motor gives zero thrust
        plane->thrust_scale = 0;
        // vtol motors start at 2
        motor_offset = 2;
    } else if (strstr(frame_str, "cl84")) {
        frame_type = "tilttri";
        // fwd motor gives zero thrust
        plane->thrust_scale = 0;
    } else if (strstr(frame_str, "-copter_tailsitter")) {
        frame_type = "+";
        plane->copter_tailsitter = true;
        aircraft->ground_behavior = GROUND_BEHAVIOR_TAILSITTER;
        plane->thrust_scale *= 1.5;
    }

    quadplane->frame = sim_frame_find_frame(frame_type);
    if (quadplane->frame == NULL) {
        printf("Failed to find frame '%s'\n", frame_type);
        exit(1);
    }

    if (strstr(frame_str, "cl84")) {
        // setup retract servos at front
        quadplane->frame->motors[0].servo_type = SERVO_RETRACT;
        quadplane->frame->motors[0].servo_rate = 7*60.0f/90; // 7 seconds to change
        quadplane->frame->motors[1].servo_type = SERVO_RETRACT;
        quadplane->frame->motors[1].servo_rate = 7*60.0f/90; // 7 seconds to change
    }

    // leave first 4 servos free for plane
    quadplane->frame->motor_offset = motor_offset;

    // we use zero terminal velocity to let the plane model handle the drag
    sim_frame_init(quadplane->frame, frame_str, &aircraft->battery);

    // increase mass for plane components
    aircraft->mass = sim_frame_get_mass(quadplane->frame) * 1.5f;
    sim_frame_set_mass(quadplane->frame, aircraft->mass);

    aircraft->lock_step_scheduled = true;
}

sim_aircraft_t sim_quadplane_create(const char *frame_str)
{
    sim_quadplane_t quadplane = (sim_quadplane_t)rt_malloc(sizeof(struct sim_quadplane));

    if (!quadplane) {
        return NULL;
    }

    sim_quadplane_ctor(quadplane, frame_str);

    return (sim_aircraft_t)quadplane;
}

/*
  update the quadplane simulation by one time step
 */
void sim_quadplane_update(sim_aircraft_t aircraft, const struct sitl_input *input)
{
    sim_quadplane_t quadplane = (sim_quadplane_t)aircraft;
    sim_plane_t plane = &quadplane->plane;

    // get wind vector setup
    sim_aircraft_update_wind(aircraft, input);

    // first plane forces
    Vector3f_t rot_accel;
    sim_plane_calculate_forces(plane, input, &rot_accel);

    // now quad forces
    Vector3f_t quad_rot_accel = VECTOR3F_DEFAULT_VALUE;;
    Vector3f_t quad_accel_body = VECTOR3F_DEFAULT_VALUE;;

    aircraft->motor_mask |= ((1U<<quadplane->frame->num_motors)-1U) << quadplane->frame->motor_offset;
    sim_frame_calculate_forces(quadplane->frame, aircraft, input, &quad_rot_accel, &quad_accel_body, aircraft->rpm, false);

    // rotate frames for copter tailsitters
    if (plane->copter_tailsitter) {
        vec3_rotate(&quad_rot_accel, ROTATION_PITCH_270);
        vec3_rotate(&quad_accel_body, ROTATION_PITCH_270);
    }

    // estimate voltage and current
    sim_frame_current_and_voltage(quadplane->frame, &aircraft->battery_voltage, &aircraft->battery_current);

    sim_battery_set_current(&aircraft->battery , aircraft->battery_current);

    float throttle;
    if (plane->reverse_thrust) {
        throttle = sim_aircraft_filtered_servo_angle(aircraft, input, 2);
    } else {
        throttle = sim_aircraft_filtered_servo_range(aircraft, input, 2);
    }
    // assume 20A at full fwd throttle
    throttle = fabsf(throttle);
    aircraft->battery_current += 20 * throttle;

    rot_accel.x += quad_rot_accel.x;
    rot_accel.y += quad_rot_accel.y;
    rot_accel.z += quad_rot_accel.z;

    aircraft->accel_body.x += quad_accel_body.x;
    aircraft->accel_body.y += quad_accel_body.y;
    aircraft->accel_body.z += quad_accel_body.z;

    sim_aircraft_update_dynamics(aircraft, &rot_accel);
    sim_aircraft_update_external_payload(aircraft, input);

    // update lat/lon/altitude
    sim_aircraft_update_position(aircraft);
    sim_aircraft_time_advance(aircraft);

    // update magnetic field
    sim_aircraft_update_mag_field_bf(aircraft);
}

/*------------------------------------test------------------------------------*/


